using System;
using System.Collections.Generic;
using System.Text;
using Blimpbots;
using Blimpbots.Recognition;
using System.IO;

/* BLIMP WIRING:
 *   
 *     _ ^forward
 *    / \
 *   |   |
 * L |   | R
 *   |   |
 *    \_/
 *     
 * connect motors such that:
 * With the board chip side up, PIC at top:
 * Motor one (left connection) is the left motor on the blimp
 *   red wire is above black wire
 * Motor two (top connection) is the vertical motor on the blimp
 *   red wire is left of black wire
 * Motor three (right connection) is the right motor on the blimp
 *   red wire is above black wire
 */

/* BLIMP ORIENTATION:
 * 
 * an orientation of '0' means the blimp is facing the +x direction
 * +/-pi : facing -x
 * pi/2 : facing +y
 * -pi/2 : facing -y
 * 
 */

namespace Blimpbots.Control {
    class BlimpData {

        public bool motorsDisabled = false;
        public Blimp blimpLink;
        public TwoDimBlimpController twoDimControl;
        public OOBTwoDimBlimpController oob2DControl;
        public VerticalBlimpController verticalControl;
        public Point3D lastSampledLocation;
        public Point3D lastProjectedLocation;
        public double lastSampledOrientation;
        public double lastProjectedOrientation;
        public double lastSampledZ;
        public DateTime lastSampleTime;
        public string name;
    }
    
    class GlobalBlimpControl
    {

        #region Blimp Port Lookup

        Dictionary<string, string> blimpPorts = null;

        Dictionary<string, string> ParseBlimpPortAssignments(string filename)
        {
            Dictionary<string, string> BlimpPorts = new Dictionary<string, string>();
            StreamReader sr;
            try
            {
                sr = new StreamReader(filename);
                string row;
                while ((row = sr.ReadLine()) != null)
                {
                    string[] cols = row.Split(new char[] { ',' });
                    if (cols.Length >= 2) BlimpPorts[cols[0]] = cols[1];
                }
            }
            catch
            {
                throw new Exception("Error reading port assignment file. ");
            }
            return BlimpPorts;
        }

        #endregion

        private bool disableBlimps;

        private PolygonWorld world;

        private Dictionary<string, BlimpData> BlimpDict 
            = new Dictionary<string, BlimpData>();

        private void report(String name, String s) {
            Console.WriteLine("GBC_" + name + ": " + s);
            return;
        }

        public List<Line2D> getWorldLines() {
            return world.getWorldLines();
        }

        public List<Line2D> getBlimpLines() {
            return world.getBlimpLines();
        }
        private double blimpLength, blimpWidth, blimpHeight, zSetPoint;
        public GlobalBlimpControl(bool disableBlimps, double _blimpLength, double _blimpWidth, double _blimpHeight, double _zSetPoint, Point2D _center,
            out double xMin, out double xMax, out double yMin, out double yMax) {

            blimpPorts = ParseBlimpPortAssignments("blimp_ports.csv");

            this.disableBlimps = disableBlimps;
            
            // world settings
            string filename = "atriumbounds.svg";

            blimpWidth = _blimpWidth;
            blimpHeight = _blimpHeight;
            blimpLength = _blimpLength;

            zSetPoint = _zSetPoint;

            world = new PolygonWorld(filename, blimpWidth, blimpLength, _center);
            xMin = world.xMin;
            xMax = world.xMax;
            yMin = world.yMin;
            yMax = world.yMax;
        }

        public void updateBlimp(String name, Point3D location, double orientation, DateTime sampleTime) {

            BlimpData blimp = null;
            BlimpDict.TryGetValue(name, out blimp);

            if (blimp == null) {

                Console.WriteLine("Constructing blimp {0}", name);

                blimp = new BlimpData();

                if (!disableBlimps && blimpPorts.ContainsKey(name))
                {
                    blimp.blimpLink = new Blimp(blimpPorts[name], name);
                }

                blimp.twoDimControl = new TwoDimBlimpController(name);
                blimp.oob2DControl = new OOBTwoDimBlimpController(name);
                blimp.verticalControl = new VerticalBlimpController(name, zSetPoint, blimpHeight);
                blimp.name = name;
                BlimpDict.Add(name, blimp);

                blimp.lastSampleTime = DateTime.Now;
            }

            if (blimp.blimpLink != null)
            {
                TimeSpan delta = (DateTime.Now.Subtract(sampleTime));
                if (delta.TotalSeconds < 5)
                {
                    // flash when the detection happens
                    bool top = (delta.TotalSeconds < 1.0);
                    blimp.blimpLink.SetLEDs(true, !top);
                }
                else  // turn off LEDs
                {
                    blimp.blimpLink.SetLEDs(false, false);
                }
            }

            orientation = TwoDimBlimpController.normalizeOrientation(orientation);

            Point3D projectedLocation;
            double projectedOrientation;

           // projectBlimp(delay, blimp, sampleTime, location, orientation, 
           //     out projectedLocation, out projectedOrientation);

            projectedOrientation = orientation;
            projectedLocation = location;

            double rotationalVelocity;

            double zVelocity;

            if (toSeconds(sampleTime) - toSeconds(blimp.lastSampleTime) != 0)
            {
                zVelocity = (location.Z - blimp.lastSampledZ)
                                / ((toSeconds(sampleTime) - toSeconds(blimp.lastSampleTime)));
                rotationalVelocity = (orientation - blimp.lastSampledOrientation)
                                      / ((toSeconds(sampleTime) - toSeconds(blimp.lastSampleTime)));
            }
            else
            {
                zVelocity = 0;
                rotationalVelocity = 0;
            }

            // set these after projectBlimp, because it needs to use the previous values
            // to determine rate of change


            bool oob;
            double oobUrgency;

            double orientationChange = -1*world.updateBlimp(name, new Point2D(projectedLocation.X, projectedLocation.Y),
                                               projectedOrientation, rotationalVelocity, 
                                               out oob, out oobUrgency);

            double leftMotorSetting, rightMotorSetting;
            double desiredOrientation;
            
            desiredOrientation = projectedOrientation + orientationChange;

            TwoDimBlimpController.normalizeOrientation(desiredOrientation);


            if (oob) {
                blimp.oob2DControl.update(projectedOrientation, desiredOrientation, oobUrgency,
                                   out leftMotorSetting, out rightMotorSetting);
            }
            else {
                report(name, "not oob: current: " + projectedOrientation + " desired: " + desiredOrientation);
                blimp.twoDimControl.update(projectedOrientation, desiredOrientation,
                                           out leftMotorSetting, out rightMotorSetting);
            }

            if (blimp.motorsDisabled && blimp.blimpLink != null)
            {
                /* Ensure motors off */
                blimp.blimpLink.setSpeed(1, 0);
                blimp.blimpLink.setSpeed(2, 0);
                blimp.blimpLink.setSpeed(3, 0);
            }
            else if (!disableBlimps) {
                blimp.blimpLink.setFPSpeed(1, leftMotorSetting);
                blimp.blimpLink.setFPSpeed(3, rightMotorSetting);
                blimp.blimpLink.setFPSpeed(2, blimp.verticalControl.update(location.Z, zVelocity));
            }

            blimp.lastSampledLocation = location;
            blimp.lastSampledOrientation = orientation;
            blimp.lastSampledZ = location.Z;

            blimp.lastProjectedLocation = projectedLocation;
            blimp.lastProjectedOrientation = projectedOrientation;

            blimp.lastSampleTime = sampleTime;
            

            return;
        }

        public void SetMotorStatus(string blimpName, bool enabled)
        {
            if (BlimpDict.ContainsKey(blimpName))
            {
                BlimpData blimp = BlimpDict[blimpName];
                blimp.motorsDisabled = !enabled;
                if (!enabled) // if a shut down command, apply now
                {
                    if (blimp.blimpLink != null)
                    {
                        blimp.blimpLink.StopMotors();
                        blimp.blimpLink.SetLEDs(true, true); // turn on LED
                    }
                }
            }
        }

        public void RequestVoltages()
        {
            foreach(BlimpData blimp in BlimpDict.Values)
                if (!disableBlimps)
                    blimp.blimpLink.requestVoltage();
        }

        public double ReadVoltage(string name)
        {
            BlimpData bd = null;
            if (BlimpDict.TryGetValue(name, out bd))
            {
                if (bd.blimpLink != null)
                    return bd.blimpLink.Voltage;
                else
                    return 0;
            }
            return 0;
        }

        public void stopAll() {
            foreach(BlimpData blimp in BlimpDict.Values)
                if (!disableBlimps)
                    blimp.blimpLink.destruct();
            }

        private void projectBlimp(double delay, BlimpData blimp, DateTime sampleTime, Point3D location, double orientation, 
                out Point3D projectedLocation, out double projectedOrientation) {
            // simple 1-step velocity estimate / projection

            // TODO: location!

            projectedLocation = location;
           // projectedOrientation = orientation; return;
            //Point3D locationDelta = location - blimp.lastSampledOrientation;
            double orientationDelta = orientation - blimp.lastSampledOrientation;
            double timeDelta = toSeconds(sampleTime) - toSeconds(blimp.lastSampleTime);

            double exactDelay = toSeconds(DateTime.Now) - toSeconds(sampleTime);
            double orientationV = orientationDelta / timeDelta;

            //projectedOrientation = orientation + (orientationV * delay);

            projectedOrientation = orientation + (orientationV * exactDelay);
            report(blimp.name, "projected orientation: " 
                + TwoDimBlimpController.normalizeOrientation(projectedOrientation));
            report(blimp.name, "sample delay: " + exactDelay);
            report(blimp.name, "speed: " + orientationV);

        }
        private double toSeconds(DateTime t) {
            // convert a DateTime to seconds since the Unix epoch
            TimeSpan ts = (t - new DateTime(1970, 1, 1));
            return ts.TotalSeconds;
        }
    }
}
